home *** CD-ROM | disk | FTP | other *** search
/ BCI NET 2 / BCI NET 2.iso / archives / programming / source / graphicgems4.lha / GemsIV / euler_angle / EulerAngles.c next >
Encoding:
C/C++ Source or Header  |  1995-02-06  |  3.8 KB  |  118 lines
  1. /**** EulerAngles.c - Convert Euler angles to/from matrix or quat ****/
  2. /* Ken Shoemake, 1993 */
  3. #include <math.h>
  4. #include <float.h>
  5. #include "EulerAngles.h"
  6.  
  7. EulerAngles Eul_(float ai, float aj, float ah, int order)
  8. {
  9.     EulerAngles ea;
  10.     ea.x = ai; ea.y = aj; ea.z = ah;
  11.     ea.w = order;
  12.     return (ea);
  13. }
  14. /* Construct quaternion from Euler angles (in radians). */
  15. Quat Eul_ToQuat(EulerAngles ea)
  16. {
  17.     Quat qu;
  18.     double a[3], ti, tj, th, ci, cj, ch, si, sj, sh, cc, cs, sc, ss;
  19.     int i,j,k,h,n,s,f;
  20.     EulGetOrd(ea.w,i,j,k,h,n,s,f);
  21.     if (f==EulFrmR) {float t = ea.x; ea.x = ea.z; ea.z = t;}
  22.     if (n==EulParOdd) ea.y = -ea.y;
  23.     ti = ea.x*0.5; tj = ea.y*0.5; th = ea.z*0.5;
  24.     ci = cos(ti);  cj = cos(tj);  ch = cos(th);
  25.     si = sin(ti);  sj = sin(tj);  sh = sin(th);
  26.     cc = ci*ch; cs = ci*sh; sc = si*ch; ss = si*sh;
  27.     if (s==EulRepYes) {
  28.     a[i] = cj*(cs + sc);    /* Could speed up with */
  29.     a[j] = sj*(cc + ss);    /* trig identities. */
  30.     a[k] = sj*(cs - sc);
  31.     qu.w = cj*(cc - ss);
  32.     } else {
  33.     a[i] = cj*sc - sj*cs;
  34.     a[j] = cj*ss + sj*cc;
  35.     a[k] = cj*cs - sj*sc;
  36.     qu.w = cj*cc + sj*ss;
  37.     }
  38.     if (n==EulParOdd) a[j] = -a[j];
  39.     qu.x = a[X]; qu.y = a[Y]; qu.z = a[Z];
  40.     return (qu);
  41. }
  42.  
  43. /* Construct matrix from Euler angles (in radians). */
  44. void Eul_ToHMatrix(EulerAngles ea, HMatrix M)
  45. {
  46.     double ti, tj, th, ci, cj, ch, si, sj, sh, cc, cs, sc, ss;
  47.     int i,j,k,h,n,s,f;
  48.     EulGetOrd(ea.w,i,j,k,h,n,s,f);
  49.     if (f==EulFrmR) {float t = ea.x; ea.x = ea.z; ea.z = t;}
  50.     if (n==EulParOdd) {ea.x = -ea.x; ea.y = -ea.y; ea.z = -ea.z;}
  51.     ti = ea.x;      tj = ea.y;    th = ea.z;
  52.     ci = cos(ti); cj = cos(tj); ch = cos(th);
  53.     si = sin(ti); sj = sin(tj); sh = sin(th);
  54.     cc = ci*ch; cs = ci*sh; sc = si*ch; ss = si*sh;
  55.     if (s==EulRepYes) {
  56.     M[i][i] = cj;      M[i][j] =  sj*si;    M[i][k] =  sj*ci;
  57.     M[j][i] = sj*sh;  M[j][j] = -cj*ss+cc; M[j][k] = -cj*cs-sc;
  58.     M[k][i] = -sj*ch; M[k][j] =  cj*sc+cs; M[k][k] =  cj*cc-ss;
  59.     } else {
  60.     M[i][i] = cj*ch; M[i][j] = sj*sc-cs; M[i][k] = sj*cc+ss;
  61.     M[j][i] = cj*sh; M[j][j] = sj*ss+cc; M[j][k] = sj*cs-sc;
  62.     M[k][i] = -sj;     M[k][j] = cj*si;    M[k][k] = cj*ci;
  63.     }
  64.     M[W][X]=M[W][Y]=M[W][Z]=M[X][W]=M[Y][W]=M[Z][W]=0.0; M[W][W]=1.0;
  65. }
  66.  
  67. /* Convert matrix to Euler angles (in radians). */
  68. EulerAngles Eul_FromHMatrix(HMatrix M, int order)
  69. {
  70.     EulerAngles ea;
  71.     int i,j,k,h,n,s,f;
  72.     EulGetOrd(order,i,j,k,h,n,s,f);
  73.     if (s==EulRepYes) {
  74.     double sy = sqrt(M[i][j]*M[i][j] + M[i][k]*M[i][k]);
  75.     if (sy > 16*FLT_EPSILON) {
  76.         ea.x = atan2(M[i][j], M[i][k]);
  77.         ea.y = atan2(sy, M[i][i]);
  78.         ea.z = atan2(M[j][i], -M[k][i]);
  79.     } else {
  80.         ea.x = atan2(-M[j][k], M[j][j]);
  81.         ea.y = atan2(sy, M[i][i]);
  82.         ea.z = 0;
  83.     }
  84.     } else {
  85.     double cy = sqrt(M[i][i]*M[i][i] + M[j][i]*M[j][i]);
  86.     if (cy > 16*FLT_EPSILON) {
  87.         ea.x = atan2(M[k][j], M[k][k]);
  88.         ea.y = atan2(-M[k][i], cy);
  89.         ea.z = atan2(M[j][i], M[i][i]);
  90.     } else {
  91.         ea.x = atan2(-M[j][k], M[j][j]);
  92.         ea.y = atan2(-M[k][i], cy);
  93.         ea.z = 0;
  94.     }
  95.     }
  96.     if (n==EulParOdd) {ea.x = -ea.x; ea.y = - ea.y; ea.z = -ea.z;}
  97.     if (f==EulFrmR) {float t = ea.x; ea.x = ea.z; ea.z = t;}
  98.     ea.w = order;
  99.     return (ea);
  100. }
  101.  
  102. /* Convert quaternion to Euler angles (in radians). */
  103. EulerAngles Eul_FromQuat(Quat q, int order)
  104. {
  105.     HMatrix M;
  106.     double Nq = q.x*q.x+q.y*q.y+q.z*q.z+q.w*q.w;
  107.     double s = (Nq > 0.0) ? (2.0 / Nq) : 0.0;
  108.     double xs = q.x*s,      ys = q.y*s,     zs = q.z*s;
  109.     double wx = q.w*xs,      wy = q.w*ys,     wz = q.w*zs;
  110.     double xx = q.x*xs,      xy = q.x*ys,     xz = q.x*zs;
  111.     double yy = q.y*ys,      yz = q.y*zs,     zz = q.z*zs;
  112.     M[X][X] = 1.0 - (yy + zz); M[X][Y] = xy - wz; M[X][Z] = xz + wy;
  113.     M[Y][X] = xy + wz; M[Y][Y] = 1.0 - (xx + zz); M[Y][Z] = yz - wx;
  114.     M[Z][X] = xz - wy; M[Z][Y] = yz + wx; M[Z][Z] = 1.0 - (xx + yy);
  115.     M[W][X]=M[W][Y]=M[W][Z]=M[X][W]=M[Y][W]=M[Z][W]=0.0; M[W][W]=1.0;
  116.     return (Eul_FromHMatrix(M, order));
  117. }
  118.